/*
 *------------------------------------------------------------------------------
 * @File      :    tp2815_mipi.c
 * @Date      :    2021-3-20
 * @Author    :    lomboswer <lomboswer@lombotech.com>
 * @Brief     :    Source file for MDP(Media Development Platform).
 *
 * Copyright (C) 2020-2021, LomboTech Co.Ltd. All rights reserved.
 *------------------------------------------------------------------------------
 */
#include "ei_comm_camera.h"
#include "camera_usr.h"
#include <pthread.h>
#include <stdlib.h>

#ifdef __cplusplus
#if __cplusplus
extern "C" {
#endif
#endif /* __cplusplus */

#define SNS_ID 2815
#define SNS_NAME  "tp2815_mipi"
#define DEV_TYPE MCSI

static SNS_STATE_S g_sastSns[VISS_MAX_DEV_NUM];

enum{
    TYPE_720P60,
    TYPE_720P50,
    TYPE_1080P30,
    TYPE_1080P25,
    TYPE_720P30,
    TYPE_720P25,
    TYPE_SD,
    TYPE_OTHER,
};

enum{
    CH_1=0,
    CH_2=1,
    CH_3=2,
    CH_4=3,
    CH_ALL=4,
    MIPI_PAGE=8,
};

enum{
    STD_TVI, //TVI
    STD_HDA, //AHD
};

enum{
    PAL,
    NTSC,
    HD25,    //720p25
    HD30,    //720p30
    FHD25,   //1080p25
    FHD30,   //1080p30
    FHD50,   //1080p50
    FHD60,   //1080p60
    QHD25,   //2560x1440p25
    QHD30,   //2560x1440p30
    UVGA25,  //1280x960p25, must use with MIPI_4CH4LANE_445M
    UVGA30,  //1280x960p30, must use with MIPI_4CH4LANE_445M
    HD30HDR, //special 720p30 with ISX019, must use with MIPI_4CH4LANE_396M
    HD50,    //720p50
    HD60,    //720p60
    FHD275,  //1080p27.5 
};

enum{
    MIPI_4CH4LANE_297M, //up to 4x720p25/30
    MIPI_4CH4LANE_594M, //up to 4x1080p25/30
    MIPI_4CH2LANE_594M, //up to 4x720pp25/30
    MIPI_4CH4LANE_445M, //only for 4x960p25/30
    MIPI_2CH4LANE_594M, //up to 2xQHDp25/30 or 2x1080p50/60
    MIPI_4CH4LANE_396M, //only for 4xHD30HDR
    MIPI_3CH4LANE_594M,
};

typedef struct eiCAMERA_S {
    pthread_mutex_t mutex;
} CAMERA_S;

static const SNS_REG_S s_a4chFhd25StdHda[] = {
    {0x02, 0x40},
    {0x07, 0xc0},
    {0x0b, 0xc0},
    {0x0c, 0x03},
    {0x0d, 0x50},
    {0x15, 0x03},
    {0x16, 0xd2},
    {0x17, 0x80},
    {0x18, 0x29},
    {0x19, 0x38},
    {0x1a, 0x47},
    {0x1c, 0x0a},  //1920*1080, 25fps
    {0x1d, 0x50},  //
    {0x20, 0x30},
    {0x21, 0x84},
    {0x22, 0x36},
    {0x23, 0x3c},
    {0x2b, 0x60},
    {0x2c, 0x0a},
    {0x2d, 0x30},
    {0x2e, 0x70},
    {0x30, 0x48},
    {0x31, 0xbb},
    {0x32, 0x2e},
    {0x33, 0x90},
    {0x35, 0x05},
    {0x38, 0x00},
    {0x39, 0x1C},

    {0x02, 0x44},
    {0x0d, 0x73},
    {0x15, 0x01},
    {0x16, 0xf0},
    {0x20, 0x3c},
    {0x21, 0x46},
    {0x25, 0xfe},
    {0x26, 0x0d},
    {0x2c, 0x3a},
    {0x2d, 0x54},
    {0x2e, 0x40},
    {0x30, 0xa5},
    {0x31, 0x86},
    {0x32, 0xfb},
    {0x33, 0x60},

    {0x40, 0x08}, //MIPI page
    {0x01, 0xf0},
    {0x02, 0x01},
    {0x08, 0x0f},

    {0x20, 0x44},
    {0x34, 0xe4},
    {0x15, 0x0C},
    {0x25, 0x08},
    {0x26, 0x06},
    {0x27, 0x11},
    {0x29, 0x0a},
    {0x33, 0x07},
    {0x33, 0x00},
    {0x14, 0x33},
    {0x14, 0xb3},
    {0x14, 0x33},

    {0x23, 0x02},
    {0x23, 0x00},
    {0x40, 0x08}, //MIPI page
};

static const SNS_REG_S s_a4chFhd30StdHda[] = {
    {0x02, 0x40},
    {0x07, 0xc0},
    {0x0b, 0xc0},
    {0x0c, 0x03},
    {0x0d, 0x50},
    {0x15, 0x03},
    {0x16, 0xd2},
    {0x17, 0x80},
    {0x18, 0x29},
    {0x19, 0x38},
    {0x1a, 0x47},
    {0x1c, 0x08}, //1920*1080, 30fps
    {0x1d, 0x98},
    {0x20, 0x30},
    {0x21, 0x84},
    {0x22, 0x36},
    {0x23, 0x3c},
    {0x2b, 0x60},
    {0x2c, 0x0a},
    {0x2d, 0x30},
    {0x2e, 0x70},
    {0x30, 0x48},
    {0x31, 0xbb},
    {0x32, 0x2e},
    {0x33, 0x90},
    {0x35, 0x05},
    {0x38, 0x00},
    {0x39, 0x1C},

    {0x02, 0x44},
    {0x0d, 0x72},
    {0x15, 0x01},
    {0x16, 0xf0},
    {0x20, 0x38},
    {0x21, 0x46},
    {0x25, 0xfe},
    {0x26, 0x0d},
    {0x2c, 0x3a},
    {0x2d, 0x54},
    {0x2e, 0x40},
    {0x30, 0xa5},
    {0x31, 0x95},
    {0x32, 0xe0},
    {0x33, 0x60},

    {0x40, 0x08}, //MIPI page
    {0x01, 0xf0},
    {0x02, 0x01},
    {0x08, 0x0f},

    {0x20, 0x44},
    {0x34, 0xe4},
    {0x15, 0x0C},
    {0x25, 0x08},
    {0x26, 0x06},
    {0x27, 0x11},
    {0x29, 0x0a},
    {0x33, 0x07},
    {0x33, 0x00},
    {0x14, 0x33},
    {0x14, 0xb3},
    {0x14, 0x33},

    {0x23, 0x02},
    {0x23, 0x00},
    {0x40, 0x08}, //MIPI page
};

static const SNS_REG_S Audio_pcm_8k_16bit_8chns[] = {
    /* enable audio */
    { 0x40, 0x40},
    { 0x00, 0x00}, /* AIN1 ~ 4, for POS0,2,4, 6  */
    { 0x01, 0x00},
    { 0x02, 0x01},
    { 0x03, 0x02},
    { 0x08, 0x00}, /* AIN1 ~ 4, for POS8,A,C, E  */
    { 0x09, 0x00},
    { 0x0A, 0x03},
    { 0x0B, 0x04},
    { 0x17, 0x80}, /* record 16bit i2s mode */
    { 0x1b, 0x01}, /* playback 16bit i2s */
    { 0x18, 0xE0},
    { 0x19, 0x1F}, /* 1F-> enable cascade, 0F-> disable cascade*/
    { 0x1a, 0x15}, /* first chip digital input */
    { 0x37, 0x20},
    { 0x38, 0x38},
    { 0x3E, 0x00},
    { 0x7a, 0x25},
    { 0x3d, 0x01}, /* reset */
};

static const SNS_REG_S Audio_pcm_16k_16bit_8chns[] = {
    /* enable audio */
    { 0x40, 0x40},
    { 0x00, 0x01}, /* AIN1 ~ 4, for POS0,2,4, 6  */
    { 0x02, 0x02},
    { 0x04, 0x03},
    { 0x06, 0x04},
    { 0x08, 0x05}, /* AIN1 ~ 4, for POS8,A,C, E  */
    { 0x0A, 0x06},
    { 0x0C, 0x07},
    { 0x0E, 0x08},
    { 0x17, 0x80}, /* record 16bit i2s mode */
    { 0x1b, 0x01}, /* playback 16bit i2s */
    { 0x18, 0xC1},
    { 0x19, 0x1F},
    { 0x1a, 0x15}, /* first chip digital input */
    { 0x37, 0x20},
    { 0x38, 0x38},
    { 0x3E, 0x00},
    { 0x7a, 0x25},
    { 0x3d, 0x01}, /* reset */
};

static CAMERA_POWER_ACT_S s_stPowerUpAct[] = {
    {SNS_RESET, SNS_DOWN},
    {SNS_DELAY, 30},
    {SNS_RESET, SNS_UP},
    {SNS_DELAY, 100},
};

static CAMERA_POWER_ACT_S s_sPowerDownAct[] = {
    {SNS_RESET, SNS_DOWN},
    {SNS_DELAY, 10},
};

static SNS_TYPE_ATTR_S s_astSnsAttr[] = {
    {
        .enSnsType                  = TP2815_MIPI_1920_1080_25FPS_4CH_YUV,
        .enPixelFormat              = PIX_FMT_YUV_SEMIPLANAR_420,
        .enRgbSeq                   = VISS_YUV_SEQ_C0C1C2C3,
        .stSize                     = {1920, 1080},
        .u32ModuleClk               = 594000000,
        .u32Mclock                  = 27000000,
        .enWorkMode                 = VISS_WORK_MODE_4Chn,

        .u32FrameRate               = 25,
        .astRegCfgs[0].pstRegs      = s_a4chFhd25StdHda,
        .astRegCfgs[0].u32RegsCnt   = ARRAY_SIZE(s_a4chFhd25StdHda),

        .stDevCfg.stMcsiCfg.u32PhyFreq  = 1188000000,
        .stDevCfg.stMcsiCfg.enDlanes    = VISS_LANE_NUM_4Lane,
    },
    {
        .enSnsType                  = TP2815_MIPI_1920_1080_30FPS_4CH_YUV,
        .enPixelFormat              = PIX_FMT_YUV_SEMIPLANAR_420,
        .enRgbSeq                   = VISS_YUV_SEQ_C0C1C2C3,
        .stSize                     = {1920, 1080},
        .u32ModuleClk               = 594000000,
        .u32Mclock                  = 27000000,
        .enWorkMode                 = VISS_WORK_MODE_4Chn,

        .u32FrameRate               = 30,
        .astRegCfgs[0].pstRegs      = s_a4chFhd30StdHda,
        .astRegCfgs[0].u32RegsCnt   = ARRAY_SIZE(s_a4chFhd30StdHda),

        .stDevCfg.stMcsiCfg.u32PhyFreq  = 1188000000,
        .stDevCfg.stMcsiCfg.enDlanes    = VISS_LANE_NUM_4Lane,
    },

    {
        .enSnsType                  = TP2815_MIPI_ADAPTIVE_MASHUP_4CH_YUV,
        .enPixelFormat              = PIX_FMT_YUV_SEMIPLANAR_420,
        .enRgbSeq                   = VISS_YUV_SEQ_C0C1C2C3,
        .u32ModuleClk               = 594000000,
        .u32Mclock                  = 27000000,
        .enWorkMode                 = VISS_WORK_MODE_4Chn,

        .u32FrameRate               = 30,
        .stDevCfg.stMcsiCfg.u32PhyFreq  = 1188000000,
        .stDevCfg.stMcsiCfg.enDlanes    = VISS_LANE_NUM_4Lane,
    },

    {
        .enSnsType                  = TP2815_MIPI_AHD_TVI_MASHUP_3CH_YUV,
        .enPixelFormat              = PIX_FMT_YUV_SEMIPLANAR_420,
        .enRgbSeq                   = VISS_YUV_SEQ_C0C1C2C3,
        .u32ModuleClk               = 594000000,
        .u32Mclock                  = 27000000,
        .enWorkMode                 = VISS_WORK_MODE_3Chn,
        .stSize                     = {1920, 1080},

        .u32FrameRate               = 30,
        .stDevCfg.stMcsiCfg.u32PhyFreq  = 1188000000,
        .stDevCfg.stMcsiCfg.enDlanes    = VISS_LANE_NUM_4Lane,
    },

};

static int CAMERA_SetI2cInfo(VISS_DEV dev)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    if (pstSnsState->stI2cInfo.fd > 0) {
        return EI_SUCCESS;
    }

    pstSnsState->stI2cInfo.u8RegBits = 8;
    pstSnsState->stI2cInfo.u8ValBits = 8;
    pstSnsState->stI2cInfo.u16I2cBusNum = 4;
    pstSnsState->stI2cInfo.u16I2cDevAddr = 0x44;
    pstSnsState->pcName = SNS_NAME;

    return EI_SUCCESS;
}
static EI_S32 CAMERA_Detect(VISS_DEV VissDev)
{
    EI_U8 u8Tmp[2];
    EI_S32 s32Ret = EI_SUCCESS;
    EI_U16 u16Id;
    SNS_STATE_S *pstSnsState = EI_NULL;
    I2C_INFO_S *pstI2cInfo = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);

    pstI2cInfo = &pstSnsState->stI2cInfo;

    CAMERA_CHECK_POINTER(pstI2cInfo->pfnReadReg);
    s32Ret = pstI2cInfo->pfnReadReg(pstSnsState, 0xFE, &u8Tmp[0]);
    if (s32Ret != 0) {
        PRT_VISS_ERR("%s tmp[0]=%x\n", SNS_NAME, u8Tmp[0]);
        return s32Ret;
    }
    s32Ret = pstI2cInfo->pfnReadReg(pstSnsState, 0xFF, &u8Tmp[1]);
    if (s32Ret != 0) {
        PRT_VISS_ERR("%s tmp[0]=%x,tmp[1]=%x\n", SNS_NAME, u8Tmp[0], u8Tmp[1]);
        return s32Ret;
    }
    u16Id = (u8Tmp[0] << 8) | u8Tmp[1];
    PRT_VISS_INFO("%s id: 0x%04x\n", SNS_NAME, u16Id);

    if (u16Id != 0x2855) {
        PRT_VISS_ERR("%s ID wrong! (0x%04x != 0x2855)\n", SNS_NAME, u16Id);
        return EI_FAILURE;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_Stream(VISS_DEV dev, EI_BOOL bOn)
{
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    PRT_VISS_INFO("%s CAMERA_Stream*******%d\n", SNS_NAME, bOn);

    //mipi setting
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, MIPI_PAGE); //MIPI page
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x01, 0xf0);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x01);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x08, 0x0f);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x44);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0C);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x08);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x06);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x11);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xb3);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);

    if (bOn) {
        s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x00);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("CAMERA_Stream*******%d\n", bOn);
        }
    } else {
        s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x02);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("CAMERA_Stream*******%d\n", bOn);
        }
    }

    return s32Ret;

}

/* The following functions can be modified as needed */

static int CAMERA_I2cInit(VISS_DEV dev)
{
    SNS_STATE_S *pstSnsState = EI_NULL;
    int ret = EI_SUCCESS;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    ret = CAMERA_SetI2cInfo(dev);
    CAMERA_CHECK_SUCCESS(ret);

    ret = CAMERA_I2C_Reg_Ops(&pstSnsState->stI2cInfo);
    CAMERA_CHECK_SUCCESS(ret);

    return EI_SUCCESS;
}

static int CAMERA_I2cExit(VISS_DEV dev)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    return EI_SUCCESS;
}


static EI_S32 CAMERA_PowerUp(VISS_DEV VissDev)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);

    MI_VISS_SnsPower(VissDev, 0xa0000000);
    CAMERA_SensorPowerUp(VissDev, pstSnsState);

    return 0;
}

static EI_S32 CAMERA_PowerDown(VISS_DEV VissDev)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);

    MI_VISS_SnsPower(VissDev, 0x00000000);

    CAMERA_SensorPowerDown(VissDev, pstSnsState);

    return 0;
}

static EI_S32 CAMERA_I2cWrite(VISS_DEV VissDev, EI_U32 u32Ch, EI_U32 u32Reg, EI_U32 u32Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
    SNS_STATE_S *pstSnsState = EI_NULL;
    I2C_INFO_S *pstI2cInfo = EI_NULL;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);
    pstI2cInfo = &pstSnsState->stI2cInfo;
    CAMERA_CHECK_POINTER(pstI2cInfo->pfnWriteReg);
    pStCam = (CAMERA_S *)pstSnsState->pCam;

    pthread_mutex_lock(&pStCam->mutex);
    s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, 0x40, u32Ch);
    s32Ret = pstI2cInfo->pfnWriteReg(pstSnsState, u32Reg, u32Val);
    pthread_mutex_unlock(&pStCam->mutex);

    return s32Ret;
}

static EI_S32 CAMERA_I2cRead(VISS_DEV VissDev, EI_U32 u32Ch, EI_U32 u32Reg, EI_U32 *pu32Val)
{
    EI_S32 s32Ret = EI_SUCCESS;
    SNS_STATE_S *pstSnsState = EI_NULL;
    I2C_INFO_S *pstI2cInfo = EI_NULL;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);
    pstI2cInfo = &pstSnsState->stI2cInfo;
    CAMERA_CHECK_POINTER(pstI2cInfo->pfnReadReg);
    pStCam = (CAMERA_S *)pstSnsState->pCam;

    pthread_mutex_lock(&pStCam->mutex);
    s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, 0x40, u32Ch);
    s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, u32Reg, pu32Val);
    pthread_mutex_unlock(&pStCam->mutex);

    return s32Ret;
}

/////////////////////////////////
//ch: video channel
//fmt: PAL/NTSC/HD25/HD30...
//std: STD_TVI/STD_HDA
////////////////////////////////
static void CAMERA_SetChnFormat(VISS_DEV dev, EI_U8 ch, EI_U8 fmt, EI_U8 std)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    unsigned char tmp;
    const unsigned char SYS_MODE[5]={0x01,0x02,0x04,0x08,0x0f};

    SNS_GET_CTX(dev, pstSnsState);

    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, ch);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x45, 0x01);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x06, 0x12); //default value
    if (PAL == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp |= SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x06, 0x32);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x47);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x51);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xf0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x17);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x20);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xc0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x37);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3f);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x70);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x2a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x4b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x56);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x7a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x4a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x4d);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0xfb);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x65);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x04);
    } else if (NTSC == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp |= SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x47);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xd6);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x12);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xf0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xb4);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x70);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x2a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x4b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x57);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x62);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x96);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0xcb);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x65);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x04);
    } else if (HD25 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp |= SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x19);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xd0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x07);  //1280*720, 25fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xbc);  //1280*720, 25fps

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x18);

        if (STD_HDA == std) {
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x46);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x71);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x1b);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x40);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x46);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0xfe);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x01);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x3a);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x5a);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x40);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x9e);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x20);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x10);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);
        }
    }else if (HD30 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp |= SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x19);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xd0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);  //1280*720, 30fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x72);  //1280*720, 30fps

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x18);

        if (STD_HDA == std) {
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x46);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x70);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x1b);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x40);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x46);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0xfe);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x01);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x3a);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x5a);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x40);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x9d);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xca);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x01);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0xd0);
        }
    } else if (FHD30 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xd2);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x29);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x47);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x08);  //1920*1080, 30fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x98);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1C);

        if (STD_HDA == std) {
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x44);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x72);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x01);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xf0);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x2a);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x38);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x46);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0xfe);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x0d);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x3a);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x54);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x40);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0xa5);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x95);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0xe0);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x60);
        }
    } else if (FHD25 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xd2);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x29);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x47);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x0a);  //1920*1080, 25fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x50);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1C);

        if (STD_HDA == std) {
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x44);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x73);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x01);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xf0);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x2a);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x3c);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x46);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0xfe);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x0d);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x3a);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x54);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x40);

            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0xa5);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x86);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0xfb);
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x60);
        }

    } else if (FHD60 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xf0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x12);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x47);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x08);  //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x96);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0xad);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x74);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x9b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0xa5);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0xe0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x68);

    } else if (FHD50 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xe2);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x27);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x47);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x0a);  //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x4e);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0xad);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x74);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x9b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0xa5);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0xe0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x68);

    } else if (QHD30 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x50);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x23);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x1b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x5a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x0c);  //2560*1440, 30fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xe2);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x50);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0xad);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x58);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x74);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x58);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x9f);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x60);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x36, 0xdc);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x48);

    } else if (QHD25 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x50);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x23);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x1b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x5a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x0f);  //2560*1440, 25fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x76);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x50);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0xad);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x58);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x74);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x58);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x9f);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x60);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x36, 0xdc);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x48);

    } else if (UVGA25 == fmt) {//960P25
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, 0xf0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x16);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x35);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x07);  //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xbc);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xba);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x14);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x36, 0x65);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1c);
    } else if (UVGA30 == fmt) {//960P30

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, 0xf0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x16);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0xa0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x35);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);  //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x72);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x43);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x3b);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x79);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x14);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x36, 0x65);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1c);
    } else if (HD30HDR == fmt) {

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, 0xf0);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x90);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xd0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x72);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xba);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x36, 0xe8);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x18);

    } else if (HD50 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x19);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xd0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x07);  //1280*720,
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0xbc);  //1280*720, 50fps

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1c);

    } else if (HD60 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x15);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x19);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0xd0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x25);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x06);  //1280*720,
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x72);  //1280*720, 60fps

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1c);

    } else if (FHD275 == fmt) {
        pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
        tmp &= ~SYS_MODE[ch];
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x40);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x07, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0b, 0xc0);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0c, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x50);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xd2);
        //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x13);
        //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0x88);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x17, 0x80);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x29);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x19, 0x38);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1a, 0x47);
        //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x08);  //1920*1080, 30fps
        //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x98);  //

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1c, 0x09);  //1920*1080, 27.5fps
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x1d, 0x60);  //


        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x84);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x22, 0x36);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x3c);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2b, 0x60);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x30);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x70);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0x48);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0xbb);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0x2e);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x90);

        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x35, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x38, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, 0x1C);
#if 0
        if (STD_HDA == std) {
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x44);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x0d, 0x72);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x01);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x16, 0xf0);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x2a);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x38);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x21, 0x46);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0xfe);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x0d);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2c, 0x3a);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2d, 0x54);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x2e, 0x40);

                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x30, 0xa5);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x31, 0x95);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x32, 0xe0);
                pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x60);
        }
#endif
    }
}

static EI_S32 CAMERA_DecoderInit(VISS_DEV dev)
{
    EI_S32 s32Ret = EI_SUCCESS;

    /* no need */

    return s32Ret;
}

static EI_S32 CAMERA_SetDecoder(VISS_DEV dev, EI_U8 u8VinCH, EI_U8 u8Type, EI_U8 u8std)
{
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    if (u8std == STD_TVI) {
        PRT_VISS_ERR("error, Set %s decoder, not support STD_TVI ", SNS_NAME);
    }


    switch (u8Type) {
        case TYPE_720P25:
            CAMERA_SetChnFormat(dev, u8VinCH, HD25, STD_HDA);
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Width = 1280;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Height = 720;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].u32FrameRate = 25;
            break;
        case TYPE_720P30:
            CAMERA_SetChnFormat(dev, u8VinCH, HD30, STD_HDA);
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Width = 1280;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Height = 720;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].u32FrameRate = 30;
            break;
        case TYPE_1080P25:
            CAMERA_SetChnFormat(dev, u8VinCH, FHD25, STD_HDA);
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Width = 1920;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Height = 1080;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].u32FrameRate = 25;
            break;
        case TYPE_1080P30:
            CAMERA_SetChnFormat(dev, u8VinCH, FHD30, STD_HDA);
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Width = 1920;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].stSize.u32Height = 1080;
            pstSnsState->pstSnsAttr->stChFormat[u8VinCH].u32FrameRate = 30;
            break;
        default:
            PRT_VISS_ERR("Error, Not support this type %d ", u8Type);
            break;
    }

    pstSnsState->enSnsInitMode = COMMON_MANUCFG;
    pstSnsState->as32SnsChFormat[u8VinCH] = u8Type;
    pstSnsState->as32SnsStd[u8VinCH] = u8std;

    return s32Ret;
}

static EI_S32 CAMERA_SetAudioVolume(VISS_DEV dev, EI_U32 u32Chn, EI_U8 u8Volume)
{
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    EI_U8 u8TmpVin12 = 0;
    EI_U8 u8TmpVin34 = 0;
    EI_U8 u8TmpVin5 = 0;
    EI_U8 u8TmpVolume = 0;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    PRT_VISS_INFO("%s audio Ch: %d, Volume: %d", SNS_NAME, u32Chn, u8Volume);

    s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, 0x40);
    if (s32Ret != EI_SUCCESS) {
        PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
    }
    s32Ret = pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0x39, &u8TmpVin12);
    if (s32Ret != EI_SUCCESS) {
        PRT_VISS_ERR("%s audio pfnReadReg err, %d\n", SNS_NAME, s32Ret);
    }
    s32Ret = pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0x3a, &u8TmpVin34);
    if (s32Ret != EI_SUCCESS) {
        PRT_VISS_ERR("%s audio pfnReadReg err, %d\n", SNS_NAME, s32Ret);
    }
    s32Ret = pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0x3b, &u8TmpVin5);
    if (s32Ret != EI_SUCCESS) {
        PRT_VISS_ERR("%s audio pfnReadReg err, %d\n", SNS_NAME, s32Ret);
    }

    switch (u32Chn) {
        case 0x0:
            u8TmpVolume = (u8Volume | (u8TmpVin34 & 0xF0));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
        case 0x1:
            u8TmpVolume = ((u8Volume << 4) | (u8TmpVin34 & 0x0F));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
        case 0x2:
            u8TmpVolume = (u8Volume | (u8TmpVin34 & 0xF0));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x3a, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
        case 0x3:
            u8TmpVolume = ((u8Volume << 4) | (u8TmpVin34 & 0x0F));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x3a, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
        case 0x4:
            u8TmpVolume = (u8Volume | (u8TmpVin5 & 0xF0));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x3b, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
        default:
            u8TmpVolume = (u8Volume | (u8Volume << 4));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x39, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x3a, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }

            u8TmpVolume = (u8Volume | (u8TmpVin5 & 0xF0));
            s32Ret = pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x3b, u8TmpVolume);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s audio pfnWriteReg err, %d\n", SNS_NAME, s32Ret);
            }
            break;
    }

    CAMERA_CHECK_SUCCESS(s32Ret);

    return s32Ret;
}

static void CAMERA_MipiOut(VISS_DEV dev, EI_U8 output)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);

    //mipi setting
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, MIPI_PAGE); //MIPI page
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x01, 0xf0);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x02, 0x01);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x08, 0x0f);

    if (MIPI_4CH4LANE_594M == output || MIPI_2CH4LANE_594M == output) {
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x44);
        if( MIPI_2CH4LANE_594M == output)
            pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x24);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4); //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0C);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x08);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x11);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xb3);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
    } else if (MIPI_4CH4LANE_297M == output) {
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x44);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4); //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x44);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0d);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x04);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x03);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x09);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x02);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xc4);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x44);
    } else if (MIPI_4CH2LANE_594M == output) {
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x42);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4); //output vin1&vin2
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0c);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x08);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x11);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x43);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xc3);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x43);
    } else if (MIPI_4CH4LANE_445M == output) {//only for 4x960p25/30
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x44);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4); //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x12, 0x5f);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x13, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0C);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x05);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x0d);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xb3);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
    } else if (MIPI_4CH4LANE_396M == output) {//only for Sony ISX019
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x44);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0xe4); //
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x12, 0x6a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x13, 0x27);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0C);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x04);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x0c);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xb3);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
    } else if (MIPI_3CH4LANE_594M == output) {
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x20, 0x34);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x18, 0x8d); //VC0+VC1+VC2
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x34, 0x8d); //	VIN1+VIN2+VIN3
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x15, 0x0C);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x25, 0x08);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x26, 0x06);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x27, 0x11);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x29, 0x0a);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x07);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x33, 0x00);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0xb3);
        pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x14, 0x33);
    }
    /* Enable MIPI CSI2 output */
    //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x02);
    //pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x23, 0x00);
}

static EI_S32 CAMERA_Init(VISS_DEV dev)
{
    EI_U32 j, i, k;
    EI_S32 bank;
    const SNS_TYPE_ATTR_S *pstSnsType;
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    const EI_U8 SYS_MODE[5]={0x01,0x02,0x04,0x08,0x0f};
    EI_U8 tmp;
    EI_U8 u8Ahd720p25fpsCount = 0;
    EI_U8 u8Ahd720p30fpsCount = 0;
    EI_U8 u8Ahd1080p25fpsCount = 0;
    EI_U8 u8Ahd1080p30fpsCount = 0;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    pStCam = (CAMERA_S *)pstSnsState->pCam;
    pthread_mutex_lock(&pStCam->mutex);

    if (pstSnsState->pstSnsAttr->enSnsType == TP2815_MIPI_ADAPTIVE_MASHUP_4CH_YUV) {
        for (i = 0; i < VISS_DEV_MAX_CHN_NUM; i++) {
            u8Ahd720p25fpsCount = 0;
            u8Ahd720p30fpsCount = 0;
            u8Ahd1080p25fpsCount = 0;
            u8Ahd1080p30fpsCount = 0;
            for (k = 0; k < 10; k++) {
                s32Ret += pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, (0x00 + i));
                if (s32Ret != EI_SUCCESS) {
                    PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
                    pthread_mutex_unlock(&pStCam->mutex);
                    return s32Ret;
                }

                pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0x03, &tmp);
                ei_msleep(1);

                if ((tmp & 0x07) == TYPE_720P25) {
                    u8Ahd720p25fpsCount++;
                } else if ((tmp & 0x07) == TYPE_720P30) {
                    u8Ahd720p30fpsCount++;
                } else if ((tmp & 0x07) == TYPE_1080P25) {
                    u8Ahd1080p25fpsCount++;
                } else if ((tmp & 0x07) == TYPE_1080P30) {
                    u8Ahd1080p30fpsCount++;
                }
            }

            if (u8Ahd720p25fpsCount >= 3) { /* 720p25 must 30% accurecy ! */
                tmp = TYPE_720P25;
            } else if (u8Ahd720p30fpsCount >= 5) { /* 720p30 must 50% accurecy ! */
                tmp = TYPE_720P30;
            } else if (u8Ahd1080p25fpsCount >= 5) { /* 1080p25 must 50% accurecy ! */
                tmp = TYPE_1080P25;
            } else if (u8Ahd1080p30fpsCount >= 5) { /* 1080p30 must 50% accurecy ! */
                tmp = TYPE_1080P30;
            } else {
                tmp = 0;
                EI_PRINT("@@@ Warning! TP2815 CH%d use default config \n", i);
            }

            switch (tmp & 0x07) {
                case TYPE_720P25:
                    PRT_VISS_INFO("\n@@@ CH%d --- TYPE_720P25 \n", i);
                    CAMERA_SetChnFormat(dev, i, HD25, STD_HDA);
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Width = 1280;
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Height = 720;
                    pstSnsState->pstSnsAttr->stChFormat[i].u32FrameRate = 25;
                    break;
                case TYPE_720P30:
                    PRT_VISS_INFO("\n@@@ CH%d --- TYPE_720P30 \n", i);
                    CAMERA_SetChnFormat(dev, i, HD30, STD_HDA);
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Width = 1280;
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Height = 720;
                    pstSnsState->pstSnsAttr->stChFormat[i].u32FrameRate = 30;
                    break;
                case TYPE_1080P25:
                    PRT_VISS_INFO("\n@@@ CH%d --- TYPE_1080P25 \n", i);
                    CAMERA_SetChnFormat(dev, i, FHD25, STD_HDA);
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Width = 1920;
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Height = 1080;
                    pstSnsState->pstSnsAttr->stChFormat[i].u32FrameRate = 25;
                    break;
                case TYPE_1080P30:
                    PRT_VISS_INFO("\n@@@ CH%d --- TYPE_1080P30 \n", i);
                    CAMERA_SetChnFormat(dev, i, FHD30, STD_HDA);
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Width = 1920;
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Height = 1080;
                    pstSnsState->pstSnsAttr->stChFormat[i].u32FrameRate = 30;
                    break;
                default:
                    PRT_VISS_INFO("\n@@@ Default Config, CH%d --- TYPE_1080P30 \n", i);
                    CAMERA_SetChnFormat(dev, i, FHD30, STD_HDA);
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Width = 1920;
                    pstSnsState->pstSnsAttr->stChFormat[i].stSize.u32Height = 1080;
                    pstSnsState->pstSnsAttr->stChFormat[i].u32FrameRate = 30;
                    break;
            }
        }
        CAMERA_MipiOut(dev, MIPI_4CH4LANE_594M);
        goto OUT;
    } else if (pstSnsState->pstSnsAttr->enSnsType == TP2815_MIPI_AHD_TVI_MASHUP_3CH_YUV) {

        CAMERA_SetChnFormat(dev, CH_1, FHD275, STD_TVI);
        pstSnsState->pstSnsAttr->stChFormat[0].stSize.u32Width = 1920;
        pstSnsState->pstSnsAttr->stChFormat[0].stSize.u32Height = 1080;
        pstSnsState->pstSnsAttr->stChFormat[0].u32FrameRate = 27;

        CAMERA_SetChnFormat(dev, CH_2, FHD30, STD_HDA);
        pstSnsState->pstSnsAttr->stChFormat[1].stSize.u32Width = 1920;
        pstSnsState->pstSnsAttr->stChFormat[1].stSize.u32Height = 1080;
        pstSnsState->pstSnsAttr->stChFormat[1].u32FrameRate = 30;

        CAMERA_SetChnFormat(dev, CH_3, FHD30, STD_HDA);
        pstSnsState->pstSnsAttr->stChFormat[2].stSize.u32Width = 1920;
        pstSnsState->pstSnsAttr->stChFormat[2].stSize.u32Height = 1080;
        pstSnsState->pstSnsAttr->stChFormat[2].u32FrameRate = 30;

        CAMERA_SetChnFormat(dev, CH_4, FHD30, STD_HDA);
        pstSnsState->pstSnsAttr->stChFormat[3].stSize.u32Width = 1920;
        pstSnsState->pstSnsAttr->stChFormat[3].stSize.u32Height = 1080;
        pstSnsState->pstSnsAttr->stChFormat[3].u32FrameRate = 30;

        //CAMERA_MipiOut(dev, MIPI_3CH4LANE_594M);
        CAMERA_MipiOut(dev, MIPI_4CH4LANE_594M);
        goto OUT;
    }

    pstSnsType = pstSnsState->pstSnsAttr;
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, 0x04);
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x45, 0x01);

    pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0xf5, &tmp);
    tmp &= ~SYS_MODE[4];
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0xf5, tmp);

    for (j = 0; j < MAX_COF; j++) {
        s32Ret = pstSnsState->stI2cInfo.pfnwriteRegList(pstSnsState,
                pstSnsType->astRegCfgs[j].pstRegs, pstSnsType->astRegCfgs[j].u32RegsCnt);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s writeRegList err\n", SNS_NAME);
            pthread_mutex_unlock(&pStCam->mutex);
            return s32Ret;
        }
    }

OUT:

    pstSnsState->stI2cInfo.pfnReadReg(pstSnsState, 0x40, &bank);

    if ((pstSnsState->stAudioAttr.u32SampleRate == 8) &&
        (pstSnsState->stAudioAttr.u32BitWidth == 16) &&
        (pstSnsState->stAudioAttr.u32ChnNum == 8)) {
        s32Ret = pstSnsState->stI2cInfo.pfnwriteRegList(pstSnsState,
                    Audio_pcm_8k_16bit_8chns,
                    ARRAY_SIZE(Audio_pcm_8k_16bit_8chns));
        PRT_VISS_INFO("@@@ %s Audio: 8K 16Bit\n", SNS_NAME);
    } else if ((pstSnsState->stAudioAttr.u32SampleRate == 16) &&
        (pstSnsState->stAudioAttr.u32BitWidth == 16) &&
        (pstSnsState->stAudioAttr.u32ChnNum == 8)) {
        s32Ret = pstSnsState->stI2cInfo.pfnwriteRegList(pstSnsState,
                    Audio_pcm_16k_16bit_8chns,
                    ARRAY_SIZE(Audio_pcm_16k_16bit_8chns));
        PRT_VISS_INFO("@@@ %s Audio: 16K 16Bit\n", SNS_NAME);
    } else {
        s32Ret = pstSnsState->stI2cInfo.pfnwriteRegList(pstSnsState,
                    Audio_pcm_8k_16bit_8chns,
                    ARRAY_SIZE(Audio_pcm_8k_16bit_8chns));
        PRT_VISS_INFO("@@@ %s default Audio: 8K 16Bit\n", SNS_NAME);
    }
    if (s32Ret != EI_SUCCESS) {
        PRT_VISS_ERR("%s audio writeRegList err", SNS_NAME);
        pthread_mutex_lock(&pStCam->mutex);
        return s32Ret;
    }
    pstSnsState->stI2cInfo.pfnWriteReg(pstSnsState, 0x40, bank);

    pthread_mutex_unlock(&pStCam->mutex);

    PRT_VISS_INFO("%s writeRegList success\n", SNS_NAME);

    return s32Ret;
}

static void CAMERA_Exit(VISS_DEV dev)
{
    CAMERA_I2cExit(dev);

    return;
}

static EI_S32 CAMERA_SetImageMode(VISS_DEV dev, SNS_TYPE_E enSnsType)
{
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    pstSnsState->enSnsType = enSnsType;
    pstSnsState->bSyncInit = EI_FALSE;

    return EI_SUCCESS;
}

static EI_S32 CAMERA_EnumSnstype(VISS_DEV VissDev, EI_U32 u32Index, SNS_TYPE_E *peSnstype)
{
    const SNS_TYPE_ATTR_S *pstSnsType;

    if (u32Index >= ARRAY_SIZE(s_astSnsAttr))
        return EN_ERR_NO_FOUND;

    pstSnsType = &s_astSnsAttr[u32Index];

    *peSnstype = pstSnsType->enSnsType;

    return EI_SUCCESS;
}

static EI_S32 CAMERA_GetStatus(VISS_DEV VissDev, SNS_STS_S *pstSts)
{
    const SNS_TYPE_ATTR_S *pstSnsType;
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    EI_U32 i;
    I2C_INFO_S *pstI2cInfo = EI_NULL;
    EI_U8 tmp;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    pStCam = (CAMERA_S *)pstSnsState->pCam;
    pstI2cInfo = &pstSnsState->stI2cInfo;
    pstSnsType = pstSnsState->pstSnsAttr;

    pthread_mutex_lock(&pStCam->mutex);

    for (i = 0; i < pstSnsType->enWorkMode; i++) {
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, 0x40, (0x00 + i));
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
            pthread_mutex_unlock(&pStCam->mutex);
            return s32Ret;
        }

        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, 0x01, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
#if 0
        tmp = (tmp & 0x80) >> 7;
        if (!tmp) {
            s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, 0x2f, &tmp);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
                return s32Ret;
            }

            if (0x80 == tmp) {
                s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, 0x04, &tmp);
                if (s32Ret != EI_SUCCESS) {
                    PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
                    return s32Ret;
                }

                if (tmp < 0x30)
                    tmp = 0;
                else
                    tmp = 1;
            }
        }
#endif
        if ((tmp & (0x1 << 5)) && (tmp & (0x1 << 6))) {
            tmp = 0;
        } else {
            tmp = 1;
        }
        pstSts->bChnSts[i] = tmp;
        /* PRT_VISS_ERR("%s ch%d: %d \n", SNS_NAME, i, pstSts->bChnSts[i]); */

    }

    pthread_mutex_unlock(&pStCam->mutex);

    return s32Ret;
}

static EI_S32 CAMERA_GetImage(VISS_DEV VissDev, SNS_IMAGE_S *pstSts)
{
    const SNS_TYPE_ATTR_S *pstSnsType;
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    EI_U32 i;
    I2C_INFO_S *pstI2cInfo = EI_NULL;
    EI_U8 tmp;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    pStCam = (CAMERA_S *)pstSnsState->pCam;
    pstI2cInfo = &pstSnsState->stI2cInfo;
    pstSnsType = pstSnsState->pstSnsAttr;

    pthread_mutex_lock(&pStCam->mutex);

    for (i = 0; i < pstSnsType->enWorkMode; i++) {
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, 0x40, (0x00 + i));
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
            pthread_mutex_unlock(&pStCam->mutex);
            return s32Ret;
        }

        /* Get Brightness */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, BRIGHTNESS, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
        pstSts->brightness[i] = tmp;
        /* Get Contrast */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, CONTRAST, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
        pstSts->contrast[i] = tmp;
        /* Get Saturation */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, SATURATION, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
        pstSts->saturation[i] = tmp;
        /* Get Hue */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, HUE, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
        pstSts->hue[i] = tmp;
        /* Get Sharpness */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, SHARPNESS, &tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnReadReg err", SNS_NAME);
        }
        pstSts->sharpness[i] = (tmp & 0x1F);

    }

    pthread_mutex_unlock(&pStCam->mutex);

    return s32Ret;
}

static EI_S32 CAMERA_SetImage(VISS_DEV VissDev, SNS_IMAGE_S *pstSts)
{
    const SNS_TYPE_ATTR_S *pstSnsType;
    SNS_STATE_S *pstSnsState = EI_NULL;
    EI_S32 s32Ret = EI_SUCCESS;
    EI_U32 i;
    I2C_INFO_S *pstI2cInfo = EI_NULL;
    EI_U8 tmp;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(VissDev, pstSnsState);
    CAMERA_CHECK_POINTER(pstSnsState);

    pStCam = (CAMERA_S *)pstSnsState->pCam;
    pstI2cInfo = &pstSnsState->stI2cInfo;
    pstSnsType = pstSnsState->pstSnsAttr;

    pthread_mutex_lock(&pStCam->mutex);

    for (i = 0; i < pstSnsType->enWorkMode; i++) {
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, 0x40, (0x00 + i));
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
            pthread_mutex_unlock(&pStCam->mutex);
            return s32Ret;
        }

        /* Set Brightness */
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, BRIGHTNESS, pstSts->brightness[i]);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
        }
        /* Set Contrast */
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, CONTRAST, pstSts->contrast[i]);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
        }
        /* Set Saturation */
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, SATURATION, pstSts->saturation[i]);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
        }
        /* Set Hue */
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, HUE, pstSts->hue[i]);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
        }
        /* Set Sharpness */
        tmp = 0;
        s32Ret += pstI2cInfo->pfnReadReg(pstSnsState, SHARPNESS, &tmp);
        tmp &= 0xe0;
        tmp |= (pstSts->sharpness[i] & 0x1F);
        s32Ret += pstI2cInfo->pfnWriteReg(pstSnsState, SHARPNESS, tmp);
        if (s32Ret != EI_SUCCESS) {
            PRT_VISS_ERR("%s pfnWriteReg err", SNS_NAME);
        }
    }

    pthread_mutex_unlock(&pStCam->mutex);

    return s32Ret;
}

static EI_S32 CAMERA_InitSensorExpFunction(SENSOR_EXP_FUNC_S *pstSensorExpFunc)
{
    CAMERA_CHECK_POINTER(pstSensorExpFunc);

    memset(pstSensorExpFunc, 0, sizeof(SENSOR_EXP_FUNC_S));

    pstSensorExpFunc->pfnSensorPowerUp        = CAMERA_PowerUp;
    pstSensorExpFunc->pfnSensorPowerDown      = CAMERA_PowerDown;
    pstSensorExpFunc->pfnSensorDetect         = CAMERA_Detect;
    pstSensorExpFunc->pfnSensorInit           = CAMERA_Init;
    pstSensorExpFunc->pfnSensorStream         = CAMERA_Stream;
    pstSensorExpFunc->pfnSensorExit           = CAMERA_Exit;
    pstSensorExpFunc->pfnSensorSetImageMode   = CAMERA_SetImageMode;
    pstSensorExpFunc->pfnSensorEnumSnstype    = CAMERA_EnumSnstype;
    pstSensorExpFunc->pfnSensorGetStatus      = CAMERA_GetStatus;
    pstSensorExpFunc->pfnSensorDecoderInit    = CAMERA_DecoderInit;
    pstSensorExpFunc->pfnSensorSetDecoder     = CAMERA_SetDecoder;
    pstSensorExpFunc->pfnSensorSetAudioVolume = CAMERA_SetAudioVolume;
    pstSensorExpFunc->pfnSensorSetImage       = CAMERA_SetImage;
    pstSensorExpFunc->pfnSensorGetImage       = CAMERA_GetImage;
    pstSensorExpFunc->pfnSensorI2cWrite       = CAMERA_I2cWrite;
    pstSensorExpFunc->pfnSensorI2cRead        = CAMERA_I2cRead;

    return EI_SUCCESS;
}

/****************************************************************************
 * callback structure                                                       *
 ****************************************************************************/

static EI_S32 CAMERA_CtxInit(VISS_DEV dev)
{
    SNS_STATE_S *pastSnsStateCtx = EI_NULL;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(dev, pastSnsStateCtx);
    CAMERA_CHECK_POINTER(pastSnsStateCtx);

    memset(pastSnsStateCtx, 0, sizeof(SNS_STATE_S));

    pastSnsStateCtx->pCam = malloc(sizeof(CAMERA_S));
    pStCam = (CAMERA_S *)pastSnsStateCtx->pCam;
    pthread_mutex_init(&pStCam->mutex, NULL);

    pastSnsStateCtx->pstPowerUpAct      = s_stPowerUpAct;
    pastSnsStateCtx->pstPowerDownAct    = s_sPowerDownAct;
    pastSnsStateCtx->u8PwrUpActs        = ARRAY_SIZE(s_stPowerUpAct);
    pastSnsStateCtx->u8PwrDnActs        = ARRAY_SIZE(s_sPowerDownAct);

    return EI_SUCCESS;
}

static EI_S32 CAMERA_CtxExit(VISS_DEV dev)
{
    SNS_STATE_S *pastSnsStateCtx = EI_NULL;
    CAMERA_S *pStCam = EI_NULL;

    SNS_GET_CTX(dev, pastSnsStateCtx);
    CAMERA_CHECK_POINTER(pastSnsStateCtx);

    pStCam = (CAMERA_S *)pastSnsStateCtx->pCam;
    pthread_mutex_destroy(&pStCam->mutex);

    if (pStCam) {
        free(pStCam);
        pStCam = EI_NULL;
    }

    return EI_SUCCESS;
}

static EI_S32 CAMERA_RegisterVissCallback(VISS_DEV dev, SNS_TYPE_E enType)
{
    EI_S32 s32Ret;
    SENSOR_S stSns = {0};
    EI_U32 i;
    SNS_STATE_S *pstSnsState = EI_NULL;

    s32Ret = CAMERA_CtxInit(dev);
    if (EI_SUCCESS != s32Ret) {
        return EI_FAILURE;
    }

    SNS_GET_CTX(dev, pstSnsState);

    for (i = 0; i < ARRAY_SIZE(s_astSnsAttr); i++) {
        if (enType == s_astSnsAttr[i].enSnsType) {
            pstSnsState->pstSnsAttr = &s_astSnsAttr[i];
            break;
        }
    }

    pstSnsState->enSnsType = enType;
    pstSnsState->Dev = dev;

    if (i >= ARRAY_SIZE(s_astSnsAttr)) {
        PRT_VISS_ERR("%s does not support this chn configuration", SNS_NAME);
        return EI_FAILURE;
    }

    s32Ret = CAMERA_I2cInit(dev);
    if (EI_SUCCESS != s32Ret) {
        return EI_FAILURE;
    }

    stSns.stSnsAttrInfo.eSensorId = SNS_ID;
    stSns.stSnsAttrInfo.enDevType = DEV_TYPE;
    stSns.stSnsAttrInfo.pstSns = pstSnsState;

    s32Ret  = CAMERA_InitSensorExpFunction(&stSns.stSnsAttrInfo.stSnsExp);
    s32Ret |= VISS_SensorRegCallBack(dev, &stSns);
    if (EI_SUCCESS != s32Ret) {
        PRT_VISS_ERR("%s sensor register callback function failed!\n", SNS_NAME);
        return s32Ret;
    }

    s32Ret = CAMERA_OpenI2c(pstSnsState);
    CAMERA_CHECK_SUCCESS(s32Ret);

    return EI_SUCCESS;
}

static EI_S32 CAMERA_UnregisterVissCallback(VISS_DEV dev)
{
    EI_S32 s32Ret;
    SNS_STATE_S *pstSnsState = EI_NULL;

    SNS_GET_CTX(dev, pstSnsState);

    s32Ret = CAMERA_ExitI2c(pstSnsState);
    CAMERA_CHECK_SUCCESS(s32Ret);

    s32Ret = VISS_SensorUnRegCallBack(dev, SNS_ID);
    if (EI_SUCCESS != s32Ret) {
        PRT_VISS_ERR("%s sensor unregister callback function failed!\n", SNS_NAME);
        return s32Ret;
    }

    CAMERA_CtxExit(dev);

    return EI_SUCCESS;
}

static EI_S32 CAMERA_GetVissDevAttrBySns(SNS_TYPE_E enType,
    VISS_DEV_ATTR_S *pstVissDevAttr)
{
    EI_U32 i;
    const SNS_TYPE_ATTR_S *pstSnsType;

    for (i = 0; i < ARRAY_SIZE(s_astSnsAttr); i++) {
        if (enType == s_astSnsAttr[i].enSnsType) {
            pstSnsType = &s_astSnsAttr[i];
            pstVissDevAttr->stSize          = pstSnsType->stSize;
            pstVissDevAttr->enWorkMode      = pstSnsType->enWorkMode;
            pstVissDevAttr->mClock          = pstSnsType->u32Mclock;
            pstVissDevAttr->enPixelFormat   = pstSnsType->enPixelFormat;
            pstVissDevAttr->enWdrMode       = pstSnsType->enWdrMode;
            pstVissDevAttr->enIspWdrMode    = pstSnsType->enIspWdrMode;
            pstVissDevAttr->enRgbSeq        = pstSnsType->enRgbSeq;
            pstVissDevAttr->u32ModuleClk    = pstSnsType->u32ModuleClk;
            pstVissDevAttr->u32Fps          = pstSnsType->u32FrameRate;
            pstVissDevAttr->u32IspClk       = pstSnsType->u32IspClk;
            pstVissDevAttr->u32IppuClk      = pstSnsType->u32IppuClk;
            pstVissDevAttr->pcSnsName       = SNS_NAME;
            pstVissDevAttr->stDevCfg        = pstSnsType->stDevCfg;
            return EI_SUCCESS;
        }
    }

    PRT_VISS_ERR("%s does not support this dev configuration", SNS_NAME);

    return EI_FAILURE;
}

static EI_S32 CAMERA_GetVissChnAttrBySns(SNS_TYPE_E enType,
    VISS_CHN_ATTR_S *pstVissChnAttr)
{
    EI_U32 i;
    const SNS_TYPE_ATTR_S *pstSnsType;

    for (i = 0; i < ARRAY_SIZE(s_astSnsAttr); i++) {
        if (enType == s_astSnsAttr[i].enSnsType) {
            pstSnsType = &s_astSnsAttr[i];
            pstVissChnAttr->stSize          = pstSnsType->stSize;
            pstVissChnAttr->enPixelFormat   = pstSnsType->enPixelFormat;
            pstVissChnAttr->stFrameRate.s32SrcFrameRate     = pstSnsType->u32FrameRate;
            return EI_SUCCESS;
        }
    }

    PRT_VISS_ERR("%s does not support this chn configuration", SNS_NAME);

    return EI_FAILURE;

}

CAMERA_OBJ_S stCameraTp2815mipiObj = {
    .pfnRegisterVissCallback    = CAMERA_RegisterVissCallback,
    .pfnUnRegisterVissCallback  = CAMERA_UnregisterVissCallback,
    .pfnRegisterIspCallback     = EI_NULL,
    .pfnUnRegisterIspCallback   = EI_NULL,
    .pfnGetVissDevAttrBySns     = CAMERA_GetVissDevAttrBySns,
    .pfnGetVissChnAttrBySns     = CAMERA_GetVissChnAttrBySns,
};

#ifdef __cplusplus
#if __cplusplus
}
#endif
#endif /* __cplusplus */

